#include <vector>
#include <cmath>
#include <algorithm>

using namespace std;

/**
 * @brief Assembles internal element forces to global system internal forces
 * 
 * This function assembles internal forces from element level to global system.
 * Note: A minus sign is still missing in the original implementation.
 * 
 * 
 * @param FINT Global internal force array
 * @param SPRESU Pressure load array
 * @param ASEC Section force array
 * @param NSEL Section selection array
 * @param LSEL Local selection array
 * @param IORDIV Order division array
 * @param IASS Assembly flag
 * @param LLT Number of local elements
 * @param IL Element connectivity array
 * @param VL Velocity array
 * @param DICOS Direction cosines array
 * @param AREA Element area array
 * @param FORCE Element force array
 * @param FMOM Element moment array
 * @param XTWORK Work array
 * @param NSTART Starting index
 * @param DTGREL Relative time step array
 * @param XL Local coordinates array
 * @param XN Normal coordinates array
 * @param X Global coordinates array
 * @param TSNODE Thermal node array
 * @param SHEMAS Shell mass array
 * @param DELTP Delta time array
 * @param IFTMAT Thermal material flag
 * @param TFLX Thermal flux array
 * @param FLUXHT Heat flux array
 * @param TSHMOM Thick shell moment array
 * @param WORK Work array
 * @param ITSHFLAG Thick shell flag
 * @param IFINT Internal force flag
 * @param PIPSCA Pipe scale factor
 * @param PIPMAX Pipe maximum value
 * @param PIPCOU Pipe count
 * @param RAIDZ Raid Z array
 * @param ROTZ Rotation Z array
 * @param ROEL2 Rotational energy array
 * @param NSEC_C Section count
 * @param F2DINT 2D internal force array
 */
void QMOMEN(vector<double>& FINT, const vector<double>& SPRESU, vector<vector<double>>& ASEC,
            const vector<vector<int>>& NSEL, const vector<int>& LSEL, const vector<int>& IORDIV,
            int IASS, int LLT, const vector<vector<int>>& IL, const vector<vector<double>>& VL,
            const vector<vector<double>>& DICOS, const vector<double>& AREA, vector<vector<double>>& FORCE,
            vector<vector<double>>& FMOM, double& XTWORK, int NSTART, const vector<double>& DTGREL,
            const vector<vector<double>>& XL, const vector<vector<double>>& XN, const vector<vector<double>>& X,
            vector<double>& TSNODE, const vector<double>& SHEMAS, const vector<double>& DELTP,
            int IFTMAT, vector<double>& TFLX, const vector<vector<double>>& FLUXHT,
            const vector<vector<double>>& TSHMOM, const vector<double>& WORK, int ITSHFLAG,
            int IFINT, const vector<double>& PIPSCA, const vector<double>& PIPMAX, vector<double>& PIPCOU,
            const vector<double>& RAIDZ, const vector<vector<double>>& ROTZ, const vector<vector<double>>& ROEL2,
            int NSEC_C, vector<vector<double>>& F2DINT) {

    // Constants from modules
    const int NUMPC2 = 0;  // Should be defined from MATELE_COM
    const int IPROJ = 0;   // Should be defined from MATELE_COM
    const int ISUBCY = 0;  // Should be defined from MATELE_COM
    const int IITER = 0;   // Should be defined from MATELE_COM
    const int N4NODE = 0;  // Should be defined from MATELE_COM
    const int ILSI = 0;    // Should be defined from MATELE_COM
    const int NUMBAG = 0;  // Should be defined from SAFETY_COM
    const int NSEC = 0;    // Should be defined from OUTPUT_COM
    const int IPIP = 0;    // Should be defined from CONTROL_COM
    const double VELSCF = 0.0; // Should be defined from STAMP_COM

    // Local constants
    const double O4TH = 0.25;

    // Local arrays
    vector<vector<double>> GMOM(LLT, vector<double>(12));
    vector<int> LIST(LLT);
    vector<vector<double>> FMOMZ(LLT, vector<double>(4));
    vector<int> IGATH(LLT), JGATH(LLT);
    vector<double> STIFF(LLT), WORK4(LLT), DTSCF(LLT);

    if (ITSHFLAG == 0) {
        if (IPROJ != 0) {
            // Initialize FMOMZ and adjust FORCE for projection
            for (int I = 0; I < LLT; I++) {
                FMOMZ[I][0] = FMOMZ[I][1] = FMOMZ[I][2] = FMOMZ[I][3] = 0.0;
                FORCE[I][0] = -(FORCE[I][3] + FORCE[I][6] + FORCE[I][9]);
                FORCE[I][1] = -(FORCE[I][4] + FORCE[I][7] + FORCE[I][10]);
                FORCE[I][2] = -(FORCE[I][5] + FORCE[I][8] + FORCE[I][11]);
            }

            // Apply projection based on IPROJ value
            switch (IPROJ) {
                case 1:
                    FRCPR1(FMOM, FMOMZ, XN, LLT);
                    break;
                case 2:
                    FRCPR2(XL, FORCE, FMOM, FMOMZ, LLT);
                    break;
                case 3:
                    FRCPR2(XL, FORCE, FMOM, FMOMZ, LLT);
                    FRCPR1(FMOM, FMOMZ, XN, LLT);
                    FRCPR2(XL, FORCE, FMOM, FMOMZ, LLT);
                    break;
                case 4:
                    FRCPR4(FMOMZ, IL, RAIDZ, ROTZ, LLT);
                    break;
            }
        }

        // Transfer forces to global coordinates
        for (int I = 0; I < LLT; I++) {
            double S1, S2;

            // Transform forces for node 1
            S1 = FORCE[I][3];
            S2 = FORCE[I][4];
            FORCE[I][3] = DICOS[I][0]*S1 + DICOS[I][1]*S2 + DICOS[I][2]*FORCE[I][5];
            FORCE[I][4] = DICOS[I][3]*S1 + DICOS[I][4]*S2 + DICOS[I][5]*FORCE[I][5];
            FORCE[I][5] = DICOS[I][6]*S1 + DICOS[I][7]*S2 + DICOS[I][8]*FORCE[I][5];

            // Transform forces for node 2
            S1 = FORCE[I][6];
            S2 = FORCE[I][7];
            FORCE[I][6] = DICOS[I][0]*S1 + DICOS[I][1]*S2 + DICOS[I][2]*FORCE[I][8];
            FORCE[I][7] = DICOS[I][3]*S1 + DICOS[I][4]*S2 + DICOS[I][5]*FORCE[I][8];
            FORCE[I][8] = DICOS[I][6]*S1 + DICOS[I][7]*S2 + DICOS[I][8]*FORCE[I][8];

            // Transform forces for node 3
            S1 = FORCE[I][9];
            S2 = FORCE[I][10];
            FORCE[I][9]  = DICOS[I][0]*S1 + DICOS[I][1]*S2 + DICOS[I][2]*FORCE[I][11];
            FORCE[I][10] = DICOS[I][3]*S1 + DICOS[I][4]*S2 + DICOS[I][5]*FORCE[I][11];
            FORCE[I][11] = DICOS[I][6]*S1 + DICOS[I][7]*S2 + DICOS[I][8]*FORCE[I][11];

            // Compute other forces by equilibrium equation
            FORCE[I][0] = -(FORCE[I][3] + FORCE[I][6] + FORCE[I][9]);
            FORCE[I][1] = -(FORCE[I][4] + FORCE[I][7] + FORCE[I][10]);
            FORCE[I][2] = -(FORCE[I][5] + FORCE[I][8] + FORCE[I][11]);
        }

        // Transform moments to global system
        if (IPROJ != 0) {
            for (int I = 0; I < LLT; I++) {
                GMOM[I][0] = DICOS[I][0]*FMOM[I][0] + DICOS[I][1]*FMOM[I][1] + DICOS[I][2]*FMOMZ[I][0];
                GMOM[I][1] = DICOS[I][3]*FMOM[I][0] + DICOS[I][4]*FMOM[I][1] + DICOS[I][5]*FMOMZ[I][0];
                GMOM[I][2] = DICOS[I][6]*FMOM[I][0] + DICOS[I][7]*FMOM[I][1] + DICOS[I][8]*FMOMZ[I][0];

                GMOM[I][3] = DICOS[I][0]*FMOM[I][2] + DICOS[I][1]*FMOM[I][3] + DICOS[I][2]*FMOMZ[I][1];
                GMOM[I][4] = DICOS[I][3]*FMOM[I][2] + DICOS[I][4]*FMOM[I][3] + DICOS[I][5]*FMOMZ[I][1];
                GMOM[I][5] = DICOS[I][6]*FMOM[I][2] + DICOS[I][7]*FMOM[I][3] + DICOS[I][8]*FMOMZ[I][1];

                GMOM[I][6] = DICOS[I][0]*FMOM[I][4] + DICOS[I][1]*FMOM[I][5] + DICOS[I][2]*FMOMZ[I][2];
                GMOM[I][7] = DICOS[I][3]*FMOM[I][4] + DICOS[I][4]*FMOM[I][5] + DICOS[I][5]*FMOMZ[I][2];
                GMOM[I][8] = DICOS[I][6]*FMOM[I][4] + DICOS[I][7]*FMOM[I][5] + DICOS[I][8]*FMOMZ[I][2];

                GMOM[I][9]  = DICOS[I][0]*FMOM[I][6] + DICOS[I][1]*FMOM[I][7] + DICOS[I][2]*FMOMZ[I][3];
                GMOM[I][10] = DICOS[I][3]*FMOM[I][6] + DICOS[I][4]*FMOM[I][7] + DICOS[I][5]*FMOMZ[I][3];
                GMOM[I][11] = DICOS[I][6]*FMOM[I][6] + DICOS[I][7]*FMOM[I][7] + DICOS[I][8]*FMOMZ[I][3];
            }
        } else {
            for (int I = 0; I < LLT; I++) {
                GMOM[I][0] = DICOS[I][0]*FMOM[I][0] + DICOS[I][1]*FMOM[I][1];
                GMOM[I][1] = DICOS[I][3]*FMOM[I][0] + DICOS[I][4]*FMOM[I][1];
                GMOM[I][2] = DICOS[I][6]*FMOM[I][0] + DICOS[I][7]*FMOM[I][1];

                GMOM[I][3] = DICOS[I][0]*FMOM[I][2] + DICOS[I][1]*FMOM[I][3];
                GMOM[I][4] = DICOS[I][3]*FMOM[I][2] + DICOS[I][4]*FMOM[I][3];
                GMOM[I][5] = DICOS[I][6]*FMOM[I][2] + DICOS[I][7]*FMOM[I][3];

                GMOM[I][6] = DICOS[I][0]*FMOM[I][4] + DICOS[I][1]*FMOM[I][5];
                GMOM[I][7] = DICOS[I][3]*FMOM[I][4] + DICOS[I][4]*FMOM[I][5];
                GMOM[I][8] = DICOS[I][6]*FMOM[I][4] + DICOS[I][7]*FMOM[I][5];

                GMOM[I][9]  = DICOS[I][0]*FMOM[I][6] + DICOS[I][1]*FMOM[I][7];
                GMOM[I][10] = DICOS[I][3]*FMOM[I][6] + DICOS[I][4]*FMOM[I][7];
                GMOM[I][11] = DICOS[I][6]*FMOM[I][6] + DICOS[I][7]*FMOM[I][7];
            }
        }
    } else {
        // Treatment of thick shell forces (ITSHFLAG = 1)
        for (int I = 0; I < LLT; I++) {
            for (int J = 0; J < 12; J++) {
                GMOM[I][J] = TSHMOM[I][J];
            }
        }
    }

    // Assemble section forces
    for (int IS = 0; IS < NSEC; IS++) {
        int NSEL2 = NSEL[1][IS];
        if (NSEL2 == 0) continue;

        int L0 = NSEL[12][IS] - 1;
        int L1 = L0 + NSEL2;
        double SUMX = 0.0, SUMY = 0.0, SUMZ = 0.0;
        double XMOM = 0.0, YMOM = 0.0, ZMOM = 0.0;

        // Gather elements belonging to this section
        int NGTHR = 0;
        for (int LS = 0; LS < NSEL2; LS++) {
            int LSG = LSEL[L0 + LS];
            int LSL = IORDIV[LSG] + 1 - NSTART;
            if (0 <= LSL && LSL < LLT) {
                IGATH[NGTHR] = LS;
                JGATH[NGTHR] = LSL;
                NGTHR++;
            }
        }

        if (NGTHR == 0) continue;

        // Process gathered elements
        for (int IG = 0; IG < NGTHR; IG++) {
            int LS = IGATH[IG];
            int LSL = JGATH[IG];
            int KODE = LSEL[L1 + LS];

            if (KODE >= 8) {
                KODE -= 8;
                SUMX += FORCE[LSL][0];
                SUMY += FORCE[LSL][1];
                SUMZ += FORCE[LSL][2];
                double XX = X[0][IL[LSL][0]] - ASEC[6][IS];
                double YY = X[1][IL[LSL][0]] - ASEC[7][IS];
                double ZZ = X[2][IL[LSL][0]] - ASEC[8][IS];
                XMOM += YY*FORCE[LSL][2] - ZZ*FORCE[LSL][1] + GMOM[LSL][0];
                YMOM += ZZ*FORCE[LSL][0] - XX*FORCE[LSL][2] + GMOM[LSL][1];
                ZMOM += XX*FORCE[LSL][1] - YY*FORCE[LSL][0] + GMOM[LSL][2];
            }

            if (KODE >= 4) {
                KODE -= 4;
                SUMX += FORCE[LSL][3];
                SUMY += FORCE[LSL][4];
                SUMZ += FORCE[LSL][5];
                double XX = X[0][IL[LSL][1]] - ASEC[6][IS];
                double YY = X[1][IL[LSL][1]] - ASEC[7][IS];
                double ZZ = X[2][IL[LSL][1]] - ASEC[8][IS];
                XMOM += YY*FORCE[LSL][5] - ZZ*FORCE[LSL][4] + GMOM[LSL][3];
                YMOM += ZZ*FORCE[LSL][3] - XX*FORCE[LSL][5] + GMOM[LSL][4];
                ZMOM += XX*FORCE[LSL][4] - YY*FORCE[LSL][3] + GMOM[LSL][5];
            }

            if (KODE >= 2) {
                KODE -= 2;
                SUMX += FORCE[LSL][6];
                SUMY += FORCE[LSL][7];
                SUMZ += FORCE[LSL][8];
                double XX = X[0][IL[LSL][2]] - ASEC[6][IS];
                double YY = X[1][IL[LSL][2]] - ASEC[7][IS];
                double ZZ = X[2][IL[LSL][2]] - ASEC[8][IS];
                XMOM += YY*FORCE[LSL][8] - ZZ*FORCE[LSL][7] + GMOM[LSL][6];
                YMOM += ZZ*FORCE[LSL][6] - XX*FORCE[LSL][8] + GMOM[LSL][7];
                ZMOM += XX*FORCE[LSL][7] - YY*FORCE[LSL][6] + GMOM[LSL][8];
            }

            if (KODE >= 1) {
                SUMX += FORCE[LSL][9];
                SUMY += FORCE[LSL][10];
                SUMZ += FORCE[LSL][11];
                double XX = X[0][IL[LSL][3]] - ASEC[6][IS];
                double YY = X[1][IL[LSL][3]] - ASEC[7][IS];
                double ZZ = X[2][IL[LSL][3]] - ASEC[8][IS];
                XMOM += YY*FORCE[LSL][11] - ZZ*FORCE[LSL][10] + GMOM[LSL][9];
                YMOM += ZZ*FORCE[LSL][9] - XX*FORCE[LSL][11] + GMOM[LSL][10];
                ZMOM += XX*FORCE[LSL][10] - YY*FORCE[LSL][9] + GMOM[LSL][11];
            }
        }

        // Update section forces (with thread-safe lock)
        ASEC[0][IS] -= SUMX;
        ASEC[1][IS] -= SUMY;
        ASEC[2][IS] -= SUMZ;
        ASEC[3][IS] -= XMOM;
        ASEC[4][IS] -= YMOM;
        ASEC[5][IS] -= ZMOM;
    }

    // Pressure load
    if (NUMPC2 != 0 || NUMBAG != 0) {
        int ICOUN = 0;
        for (int I = 0; I < LLT; I++) {
            if (SPRESU[IL[I][5]] != 0.0) {
                LIST[ICOUN] = I;
                ICOUN++;
            }
        }

        if (ICOUN == LLT) {
            for (int I = 0; I < LLT; I++) {
                double PR = -0.25 * AREA[I] * SPRESU[IL[I][5]];
                XTWORK -= PR * DTGREL[I] * (VL[I][2] + VL[I][5] + VL[I][8] + VL[I][11]);
                double S1 = PR * DICOS[I][2];
                double S2 = PR * DICOS[I][5];
                double S3 = PR * DICOS[I][8];
                
                for (int J = 0; J < 4; J++) {
                    FORCE[I][3*J]   += S1;
                    FORCE[I][3*J+1] += S2;
                    FORCE[I][3*J+2] += S3;
                }
            }
        } else {
            for (int J = 0; J < ICOUN; J++) {
                int I = LIST[J];
                double PR = -0.25 * AREA[I] * SPRESU[IL[I][5]];
                XTWORK -= PR * DTGREL[I] * (VL[I][2] + VL[I][5] + VL[I][8] + VL[I][11]);
                double S1 = PR * DICOS[I][2];
                double S2 = PR * DICOS[I][5];
                double S3 = PR * DICOS[I][8];
                
                for (int K = 0; K < 4; K++) {
                    FORCE[I][3*K]   += S1;
                    FORCE[I][3*K+1] += S2;
                    FORCE[I][3*K+2] += S3;
                }
            }
        }
    }

    // Compute stiffness
    if (ISUBCY != 0 && IITER != 1) {
        fill(STIFF.begin(), STIFF.end(), 0.0);
    } else {
        for (int I = 0; I < LLT; I++) {
            double DIV = DELTP[I] * ROEL2[0][IL[I][5]];
            STIFF[I] = 2.0 * SHEMAS[IL[I][5]] / (DIV * DIV);
        }
    }

    // Pipe-specific handling or standard assembly
    if (IPIP != 0) {
        PIPE6DOFTUNE4(IASS, LLT, IL, FORCE, GMOM, STIFF, IFINT, PIPSCA, PIPMAX, PIPCOU);
    } else {
        // Standard assembly with thread-safe lock
        if (IASS == 1) {
            for (int I = 0; I < LLT; I++) {
                // Node 1
                int J = (IL[I][0] - 1) * 6;
                FINT[J]   -= FORCE[I][0];
                FINT[J+1] -= FORCE[I][1];
                FINT[J+2] -= FORCE[I][2];
                FINT[J+3] -= GMOM[I][0];
                FINT[J+4] -= GMOM[I][1];
                FINT[J+5] -= GMOM[I][2];
                TSNODE[IL[I][0]] += STIFF[I];

                // Node 2
                J = (IL[I][1] - 1) * 6;
                FINT[J]   -= FORCE[I][3];
                FINT[J+1] -= FORCE[I][4];
                FINT[J+2] -= FORCE[I][5];
                FINT[J+3] -= GMOM[I][3];
                FINT[J+4] -= GMOM[I][4];
                FINT[J+5] -= GMOM[I][5];
                TSNODE[IL[I][1]] += STIFF[I];

                // Node 3
                J = (IL[I][2] - 1) * 6;
                FINT[J]   -= FORCE[I][6];
                FINT[J+1] -= FORCE[I][7];
                FINT[J+2] -= FORCE[I][8];
                FINT[J+3] -= GMOM[I][6];
                FINT[J+4] -= GMOM[I][7];
                FINT[J+5] -= GMOM[I][8];
                TSNODE[IL[I][2]] += STIFF[I];

                // Node 4
                J = (IL[I][3] - 1) * 6;
                FINT[J]   -= FORCE[I][9];
                FINT[J+1] -= FORCE[I][10];
                FINT[J+2] -= FORCE[I][11];
                FINT[J+3] -= GMOM[I][9];
                FINT[J+4] -= GMOM[I][10];
                FINT[J+5] -= GMOM[I][11];
                TSNODE[IL[I][3]] += STIFF[I];
            }
        } else {
            for (int I = 0; I < LLT; I++) {
                // Node 1
                int J = (IL[I][0] - 1) * 6;
                FINT[J]   -= FORCE[I][0];
                FINT[J+1] -= FORCE[I][1];
                FINT[J+2] -= FORCE[I][2];
                FINT[J+3] -= GMOM[I][0];
                FINT[J+4] -= GMOM[I][1];
                FINT[J+5] -= GMOM[I][2];

                // Node 2
                J = (IL[I][1] - 1) * 6;
                FINT[J]   -= FORCE[I][3];
                FINT[J+1] -= FORCE[I][4];
                FINT[J+2] -= FORCE[I][5];
                FINT[J+3] -= GMOM[I][3];
                FINT[J+4] -= GMOM[I][4];
                FINT[J+5] -= GMOM[I][5];

                // Node 3
                J = (IL[I][2] - 1) * 6;
                FINT[J]   -= FORCE[I][6];
                FINT[J+1] -= FORCE[I][7];
                FINT[J+2] -= FORCE[I][8];
                FINT[J+3] -= GMOM[I][6];
                FINT[J+4] -= GMOM[I][7];
                FINT[J+5] -= GMOM[I][8];

                // Node 4
                J = (IL[I][3] - 1) * 6;
                FINT[J]   -= FORCE[I][9];
                FINT[J+1] -= FORCE[I][10];
                FINT[J+2] -= FORCE[I][11];
                FINT[J+3] -= GMOM[I][9];
                FINT[J+4] -= GMOM[I][10];
                FINT[J+5] -= GMOM[I][11];

                // Update TSNODE for all nodes
                TSNODE[IL[I][0]] += STIFF[I];
                TSNODE[IL[I][1]] += STIFF[I];
                TSNODE[IL[I][2]] += STIFF[I];
                TSNODE[IL[I][3]] += STIFF[I];
            }
        }
    }

    if (ISUBCY != 0 && IITER != 1) return;

    // Assemble heat flux if thermal material
    if (IFTMAT != 0) {
        for (int I = 0; I < LLT; I++) {
            WORK4[I] = O4TH * WORK[I];
            DTSCF[I] = DTGREL[I] / VELSCF;
        }

        // With thread-safe lock
        if (IASS == 1) {
            for (int I = 0; I < LLT; I++) {
                TFLX[IL[I][0]] += FLUXHT[I][0] * DTSCF[I] + WORK4[I];
                TFLX[IL[I][1]] += FLUXHT[I][1] * DTSCF[I] + WORK4[I];
                TFLX[IL[I][2]] += FLUXHT[I][2] * DTSCF[I] + WORK4[I];
                TFLX[IL[I][3]] += FLUXHT[I][3] * DTSCF[I] + WORK4[I];
            }
        } else {
            for (int I = 0; I < LLT; I++) {
                TFLX[IL[I][0]] += FLUXHT[I][0] * DTSCF[I] + WORK4[I];
                TFLX[IL[I][1]] += FLUXHT[I][1] * DTSCF[I] + WORK4[I];
                TFLX[IL[I][2]] += FLUXHT[I][2] * DTSCF[I] + WORK4[I];
                TFLX[IL[I][3]] += FLUXHT[I][3] * DTSCF[I] + WORK4[I];
            }
        }
    }

    // Save nodal forces for energy monitoring
    if (ILSI == 1) {
        for (int I = 0; I < LLT; I++) {
            int II = IL[I][5];
            for (int J = 0; J < 4; J++) {
                F2DINT[II][6*J]   = -FORCE[I][3*J];
                F2DINT[II][6*J+1] = -FORCE[I][3*J+1];
                F2DINT[II][6*J+2] = -FORCE[I][3*J+2];
                F2DINT[II][6*J+3] = -GMOM[I][3*J];
                F2DINT[II][6*J+4] = -GMOM[I][3*J+1];
                F2DINT[II][6*J+5] = -GMOM[I][3*J+2];
            }
        }
    }
}

// Helper function declarations (would need to be implemented)
void FRCPR1(vector<vector<double>>& FMOM, vector<vector<double>>& FMOMZ, 
            const vector<vector<double>>& XN, int LLT);
void FRCPR2(const vector<vector<double>>& XL, vector<vector<double>>& FORCE, 
            vector<vector<double>>& FMOM, vector<vector<double>>& FMOMZ, int LLT);
void FRCPR4(vector<vector<double>>& FMOMZ, const vector<vector<int>>& IL, 
            const vector<double>& RAIDZ, const vector<vector<double>>& ROTZ, int LLT);
void PIPE6DOFTUNE4(int IASS, int LLT, const vector<vector<int>>& IL, 
                   vector<vector<double>>& FORCE, vector<vector<double>>& GMOM, 
                   const vector<double>& STIFF, int IFINT, const vector<double>& PIPSCA, 
                   const vector<double>& PIPMAX, vector<double>& PIPCOU);